#!/usr/bin/env python

# Copyright (c) 2018 Intel Labs.
# authors: German Ros (german.ros@intel.com)
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

""" Module with auxiliary functions. """

import math
import numpy as np
import carla

def spawn_vehilce_at_point(world:carla.World, loc: carla.Location, blueprint):
    """
    try spawn vehicle at point
    @param world: carla.World
    @param loc: carla.Location
    @param blueprint: carla.Blueprint
    return
    @world.try_spawn_actor() 
    """
    loc_map = world.get_map()
    start_wp = loc_map.get_waypoint(loc)

    spawn_points = loc_map.get_spawn_points()
    spawn_point= spawn_points[0] if spawn_points else carla.Transform()
    spawn_point.location = start_wp.transform.location
    spawn_point.rotation = start_wp.transform.rotation
    spawn_point.location.z += 5.0
    return world.try_spawn_actor(blueprint, spawn_point)

def draw_transforms(world:carla.World, transforms:list, time=10, thickness=0.03, color=carla.Color(255,0,0)):
    """
    @param world: carla.World
    @param transforms: carla.Transforms list
    @param time: float
    @param thickness: float
    @param color: carla.Color
    """
    if len(transforms)<2:
        return
    for i in range(1, len(transforms)):
        begin = transforms[i-1].location
        end = transforms[i].location
        world.debug.draw_line(begin, end, life_time=time, thickness=thickness, color=color)


def draw_way(world:carla.World, waypoints:carla.Waypoint, z=0.5, time = 10, thickness=0.1, color=carla.Color(255,255,0)):
    """
    @param world: carla.World
    @param waypoints: carla.Waypoint list
    @param z: float
    @param time: float
    @param thickness: float
    @param color: carla.Color
    """
    if len(waypoints)<2:
        return
    for i in range(len(waypoints)-1):
        begin = waypoints[i].transform.location
        end = waypoints[i+1].transform.location
        world.debug.draw_line(begin, end, life_time=time, thickness=thickness, color=color)

def draw_waypoints(world, waypoints, color=carla.Color(255,0,0), z=0.5):
    """
    Draw a list of waypoints at a certain height given in z.

        :param world: carla.world object
        :param waypoints: list or iterable container with the waypoints to draw
        :param z: height in meters
    """
    for wpt in waypoints:
        wpt_t = wpt.transform
        begin = wpt_t.location + carla.Location(z=z)
        angle = math.radians(wpt_t.rotation.yaw)
        end = begin + carla.Location(x=5*math.cos(angle), y=5*math.sin(angle))
        world.debug.draw_arrow(begin, end, color=color, arrow_size=0.3, life_time=0.05)

def draw_speed_with_tf(world, transform, speed=0.0, color=carla.Color(255,0,0), z=0.5, life_time=0.05):
    begin = transform.location + carla.Location(z=z)
    angle = math.radians(transform.rotation.yaw)
    end = begin + carla.Location(x=(5+speed)*math.cos(angle), y=(5+speed)*math.sin(angle))
    world.debug.draw_arrow(begin, end, color=color, arrow_size=0.3, life_time=life_time)

def get_speed(vehicle, unit = 'm/s'):
    """
    Compute speed of a vehicle in Km/h.

        :param vehicle: the vehicle for which speed is calculated
        :return: speed as a float in Km/h
    """
    vel = vehicle.get_velocity()
    if unit=='km/h':
        return 3.6 * math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2)
    else:
        return math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2)

def get_acceleration(vehicle, unit='m/s2'):
    """
    Compute acceleration of a vehicle in m/s2.

        :param vehicle: the vehicle for which acceleration is calculated
        :return: acceleration as a float in m/s2
    """
    acc = vehicle.get_acceleration()
    if unit == 'm/s2':
        return math.sqrt(acc.x ** 2 + acc.y ** 2 + acc.z ** 2)
    else:
        return math.sqrt(acc.x ** 2 + acc.y ** 2 + acc.z ** 2)

def is_within_distance_ahead(target_transform, current_transform, max_distance):
    """
    Check if a target object is within a certain distance in front of a reference object.

    :param target_transform: location of the target object
    :param current_transform: location of the reference object
    :param orientation: orientation of the reference object
    :param max_distance: maximum allowed distance
    :return: True if target object is within max_distance ahead of the reference object
    """
    target_vector = np.array([target_transform.location.x - current_transform.location.x, target_transform.location.y - current_transform.location.y])
    norm_target = np.linalg.norm(target_vector)

    # If the vector is too short, we can simply stop here
    if norm_target < 0.001:
        return True

    if norm_target > max_distance:
        return False

    fwd = current_transform.get_forward_vector()
    forward_vector = np.array([fwd.x, fwd.y])
    d_angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.)))

    return d_angle < 90.0

def is_within_distance(target_location, current_location, orientation, max_distance, d_angle_th_up, d_angle_th_low=0):
    """
    Check if a target object is within a certain distance from a reference object.
    A vehicle in front would be something around 0 deg, while one behind around 180 deg.

        :param target_location: location of the target object
        :param current_location: location of the reference object
        :param orientation: orientation of the reference object
        :param max_distance: maximum allowed distance
        :param d_angle_th_up: upper thereshold for angle
        :param d_angle_th_low: low thereshold for angle (optional, default is 0)
        :return: True if target object is within max_distance ahead of the reference object
    """
    target_vector = np.array([target_location.x - current_location.x, target_location.y - current_location.y])
    norm_target = np.linalg.norm(target_vector)

    # If the vector is too short, we can simply stop here
    if norm_target < 0.001:
        return True

    if norm_target > max_distance:
        return False

    forward_vector = np.array(
        [math.cos(math.radians(orientation)), math.sin(math.radians(orientation))])
    d_angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.)))

    return d_angle_th_low < d_angle < d_angle_th_up


def compute_magnitude_angle(target_location, current_location, orientation):
    """
    Compute relative angle and distance between a target_location and a current_location

        :param target_location: location of the target object
        :param current_location: location of the reference object
        :param orientation: orientation of the reference object
        :return: a tuple composed by the distance to the object and the angle between both objects
    """
    target_vector = np.array([target_location.x - current_location.x, target_location.y - current_location.y])
    norm_target = np.linalg.norm(target_vector)

    forward_vector = np.array([math.cos(math.radians(orientation)), math.sin(math.radians(orientation))])
    d_angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.)))

    return (norm_target, d_angle)


def distance_vehicle(waypoint, vehicle_transform):
    """
    Returns the 2D distance from a waypoint to a vehicle

        :param waypoint: actual waypoint
        :param vehicle_transform: transform of the target vehicle
    """
    loc = vehicle_transform.location
    x = waypoint.transform.location.x - loc.x
    y = waypoint.transform.location.y - loc.y

    return math.sqrt(x * x + y * y)


def vector(location_1, location_2):
    """
    Returns the unit vector from location_1 to location_2

        :param location_1, location_2: carla.Location objects
    """
    x = location_2.x - location_1.x
    y = location_2.y - location_1.y
    z = location_2.z - location_1.z
    norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps

    return [x / norm, y / norm, z / norm]


def compute_distance(location_1, location_2):
    """
    Euclidean distance between 3D points

        :param location_1, location_2: 3D points
    """
    x = location_2.x - location_1.x
    y = location_2.y - location_1.y
    z = location_2.z - location_1.z
    norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps
    return norm


def positive(num):
    """
    Return the given number if positive, else 0

        :param num: value to check
    """
    return num if num > 0.0 else 0.0

def get_nearest_waypoint_index(waypoints_buffer, location):
    """
    @param : waypoints_buffer  (carla.Waypoint, wayoption)
    @param : location (carla.Location)
    @return : index
    """
    old_dist = 100
    new_dist = 0
    index = -1
    for i, (wp, _) in enumerate(waypoints_buffer):
        wp_location = wp.transform.location
        new_dist = compute_distance(wp_location, location)
        if new_dist < old_dist:
            old_dist = new_dist
            index = i
    return index

def get_nearest_location_index(smooth_buffer, location):
    """
    @param : transforms_buffer  (carla.Transform)
    @param : location (carla.Location)
    @return : index
    """
    old_dist = 100
    new_dist = 0
    index = -1
    for i, (tf, _, _, _) in enumerate(smooth_buffer):
        wp_location = tf.location
        new_dist = compute_distance(wp_location, location)
        if new_dist < old_dist:
            old_dist = new_dist
            index = i
    return index

def get_curvature(wp0:carla.Waypoint, wp1:carla.Waypoint) -> float:
    """
    计算时，需要距离和转弯半径比很小，这样直接把两点的距离当成弧长
    """
    # 路点的朝向
    yaw0 = math.radians(wp0.transform.rotation.yaw)
    yaw1 = math.radians(wp1.transform.rotation.yaw)
    dyaw = yaw1 - yaw0
    while dyaw > math.pi:
        dyaw -= 2 * math.pi
    while dyaw < -math.pi:
        dyaw += 2* math.pi
    # 两点之间的距离
    dist = compute_distance(wp0.transform.location, wp1.transform.location)
    # 曲率
    return dyaw / dist

def clip_angle(dg, min_dg, max_dg):
    """
    将角度值转换到 min_dg和max_dg之间
    例如：
    [0~360)
    [-180, 180)
    """
    if dg < min_dg:
        dg += 360.0
    elif dg >= max_dg:
        dg -= 360.0
    return dg

def set_bird_view(spectator, hero):
    transform = hero.get_transform()
    transform.location.z += 50
    transform.rotation.pitch = -90
    transform.rotation.yaw -= 90
    spectator.set_transform(transform)    